#include <ros/ros.h>
#include <bsm_msgs/BSM.h>
#include <bsm_msgs/BSM_multi.h>

int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_node");
    ROS_INFO("INIT");

    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<bsm_msgs::BSM_multi>("bsm", 1);
    ros::Rate loop_rate(20);
    int count = 0;
    while (ros::ok())
    {
        bsm_msgs::BSM_multi bsm_mul;
        bsm_mul.size = 2;
        bsm_mul.data.resize(2);
        bsm_mul.data[0].idbuf = "12345678";
        bsm_mul.data[0].lon = 1200000752;
        bsm_mul.data[0].lat = 295999400;
        bsm_mul.data[0].acclon = 10.1;
        bsm_mul.data[0].acclat = 0;
        bsm_mul.data[0].accvert = 0;
        bsm_mul.data[0].accyaw = 0;
        bsm_mul.data[0].heading = 0;
        bsm_mul.data[0].speed = 10;
        bsm_mul.data[0].TransmissionState = 2;
        bsm_mul.data[1].idbuf = "12345679";
        bsm_mul.data[1].lon = 1200000760;
        bsm_mul.data[1].lat = 295999453;
        bsm_mul.data[1].acclon = 10.1;
        bsm_mul.data[1].acclat = 0;
        bsm_mul.data[1].accvert = 0;
        bsm_mul.data[1].accyaw = 0;
        bsm_mul.data[1].heading = 0;
        bsm_mul.data[1].speed = 10;
        bsm_mul.data[1].TransmissionState = 2;
        pub.publish(bsm_mul);
        ROS_INFO("PUBLISHED");
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }
}